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Abstract 

This  paper  considers  fault  detection,  isolation  and  reconfiguration  (FDIR)  for  the  localization  sensors,  in¬ 
cluding  the  dead  reckoning  and  external  sensors,  of  an  autonomous  ground  vehicle  (AGV)  designed  for  use  in 
highly  unstructured  outdoor  environments.  Ten  sensors  are  considered  in  this  research.  None  of  these  sensors 
are  identical,  but  subsets  of  them  do  have  the  ability  to  measure  or  calculate  (based  on  simple  algebra)  the  same 
kinematical  parameters.  To  improve  the  localization  accuracy,  selected  sensor  outputs  are  fused  using  Kalman 
filters.  The  fused  data  and  selected  sensor  measurements  are  then  combined  into  a  set  of  linearly  independent 
parity  equations,  which  leads  to  the  generation  of  a  bank  of  residuals.  A  fault  in  any  one  of  the  ten  sensors 
causes  a  unique  subset  of  these  residuals  to  grow,  which  allows  the  fault  to  be  detected  and  isolated.  This  al¬ 
lows  a  control  scheme  based  on  these  sensors  to  reconfigure  itself  so  that  only  the  non-faulty  sensors  are  used 
for  localization.  The  effectiveness  of  this  FDIR  scheme  is  demonstrated  in  the  context  of  a  recently  developed 
algorithm  for  maneuvering  an  AGV  in  cluttered  environments. 

Keywords:  Fault  Detection  and  Isolation ,  Fault  Reconfiguration ,  Parity  Equations ,  Lozalization ,  Kalman  Filter. 


1  Introduction 

A  growing  number  of  research  groups  around  the 
world  are  developing  autonomous  vehicle  systems  for 
various  applications  (see  [7,8]  for  example).  More  and 
more  attention  has  been  paid  to  the  issue  of  failure  and 
integrity  in  navigation  systems  of  vehicles  based  on 
sensor  technologies  [2,3,13].  Many  commercial  indus¬ 
tries  have  successfully  made  use  of  such  technology 
in  well-structured  environments  such  as  manufactur¬ 
ing  and  in  semi-structured  environments  such  as  au¬ 
tomated  agriculture.  The  research  here  focuses  on 
fault  detection,  isolation  and  reconfiguration  (FDIR) 
for  the  localization  sensors  of  wheeled  autonomous 
ground  vehicles  (AGVs),  such  as  the  experimental 
Unmanned  Vehicle  (XUV)  that  are  designed  to  oper¬ 
ate  in  highly  unstructured  outdoor  environments.  A 
variety  of  localization  sensors  may  be  used  with  such 
vehicles.  The  experimental  platform  used  here  is  the 
ATRV-Jr.  [1],  shown  in  Figure  1.  Based  primarily  on 
the  sensors  available  on  the  XUV,  ten  localization  sen¬ 
sors  are  considered.  None  of  these  sensors  is  identi¬ 
cal;  they  each  have  their  own  strengths,  weaknesses 
and  fault  modes.  However  subsets  of  these  sensors 


do  have  the  ability  to  measure  or  calculate  (based  on 
simple  algebra)  the  same  kinematical  parameters.  The 
FDIR  technique  developed  here  exploits  this  complex 
redundancy. 

The  focus  of  this  paper  is  on  the  integration  of 
Kalman  filter  based  sensor  fusion  and  a  parity  equa¬ 
tion  based  FDIR  scheme.  In  position  and  orientation 
estimation,  conventional  methods,  such  as  odometry, 
which  have  been  widely  used  for  autonomous  vehi¬ 
cles,  utilize  pulses  from  wheel  encoders  to  calculate 
the  vehicle's  current  location.  This  method  is  sim¬ 
ple  and  effective.  But  there  exist  accumulated  errors 
caused  by  wheel  slippage  and  therefore  the  robot  may 
lose  track  of  its  location  over  long  distances.  To  elim¬ 
inate  the  drawback  of  this  odometry  method  in  po¬ 
sition  and  orientation  estimation,  several  approaches 
[4, 9, 10, 14]  have  been  proposed  in  the  past  few  years. 
A  very  good  overview  is  given  in  [4]  with  the  focus 
on  the  3D  location  technique  using  active  beacon  po¬ 
sitioning  and  the  method  based  on  maps  and  world 
models.  However,  many  of  these  results  do  not  use 
the  localization  sensors  considered  here.  The  research 
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in  [9]  and  [10]  does  use  similar  sensors.  In  partic¬ 
ular,  a  magnetic  compass  is  used  in  [9]  to  calibrate 
the  orientation  drift  error  caused  by  wheel  slippage, 
thereby  resulting  in  robot  position  recovery  It  is  based 
on  a  series  of  experiments.  The  research  reported  in 
[10]  developed  a  method  named  the  UMBmark  for 
quantitatively  measuring  systematic  odometry  errors 
and,  to  a  limited  degree,  nonsystematic  errors.  The 
calculation  of  all  the  odometry  errors  terms  is  time- 
consuming  and  therefore  lacks  flexibility. 

The  research  in  [14]  uses  Kalman  filter  to  improve 
the  accuracy  of  mobilie  robot  localization.  Kalman  fil¬ 
tering  is  adopted  for  fusing  information  from  an  elec¬ 
tronic  compass  and  wheel  encoders.  This  method  is 
simple  and  effective.  As  in  [14],  the  research  in  this  pa¬ 
per  develops  a  bank  of  Kalman  filters  to  provide  more 
accurate  localization  for  FDIR  based  on  a  simplified 
kinematical  model  of  the  robot.  The  fused  outputs  of 
the  Kalman  filters  and  selected  sensor  measurements 
are  then  combined  into  a  set  of  linearly  independent 
parity  equations,  which  leads  to  the  generation  of  a 
bank  of  residuals.  A  fault  in  any  one  of  the  ten  sen¬ 
sors  causes  a  unique  subset  of  these  residuals  to  grow, 
which  allows  the  fault  to  be  detected  and  isolated. 
This  allows  a  control  scheme  based  on  these  sensors 
to  reconfigure  itself  so  that  only  the  non-faulty  sen¬ 
sors  are  used  for  localization.  The  effectiveness  of  this 
FDIR  scheme  is  demonstrated  in  the  context  of  a  re¬ 
cently  developed  algorithm  for  maneuvering  an  AGV 
in  cluttered  environments. 

The  research  developed  here  provides  an  almost 
complete  automated  FDIR  system  for  the  localization 
sensors.  However,  it  does  not  yet  include  the  integra¬ 
tion  of  DGPS  or  GPS.  The  integration  of  DGPS  or  GPS 
with  the  inertial  navigation  system  and  the  means  of 
detection  in  DGPS  or  GPS  are  planned  for  future  re¬ 
search. 


Figure  1:  The  ATRV-Jr.  with  its  Body  Frame 

This  paper  is  organized  as  follows.  Section  2  de¬ 
scribes  the  ATRV-Jr.  localization  sensors  information 
and  potential  sensor  fault  sources.  Then  it  presents  the 
respective  kinematical  equations  for  vehicle  position, 
attitude  and  velocity.  Section  3  overviews  the  FDIR 
procedures  with  the  focus  on  the  fault  residuals  and 
the  truth  table  which  shows  how  a  fault  in  any  one 
of  the  sensors  causes  a  unique  combination  of  resid¬ 
uals  to  grow.  Section  4  presents  computer  simula¬ 
tion  results  that  demonstrate  the  accuracy  of  the  FDIR 
scheme  by  navigating  an  ATRV-Jr.  robot  in  an  envi¬ 
ronment  crowded  with  obstacles.  The  navigation  al¬ 
gorithm  realizes  goal  seeking  and  obstacle  avoidance 
behaviors,  the  former  of  which  is  based  on  the  related 
localization  sensor  measurements. 

2  Localization  Sensors  and  Poten¬ 
tial  Faults 

The  ATRV-Jr.  (shown  in  Figure  1)  is  an  all-terrain 
robotic  research  platform  capable  of  autonomous  op¬ 
eration  [1].  Localization  information  is  critical  for 
the  purpose  of  navigation  and  guidance  of  the  au¬ 
tonomous  vehicle.  The  localization  sensor  suite  con¬ 
sists  of  two  wheel  encoders,  one  fluxgate  compass  in¬ 
cluding  a  tilt  sensor,  one  electrical  compass  includ¬ 
ing  a  tilt  sensor,  one  non-contact  Doppler  sensor,  one 
DGPS,  one  inertial  navigation  system,  which  includes 
three  accelerometers  and  three  gyroscopes.  Table  1 
lists  each  sensor  and  the  information  it  provides  and 
standard  fault  sources.  For  example,  the  accumulated 
errors  caused  by  wheel  slippage  is  the  typical  fault 
source  of  the  wheel  encoders. 
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Sensors 

Measured  Quantity 

Standard  Fault  Sources 

Wheel  encoder 

Velocity 

Wheel  slippage 

Fluxgate  compass 

Tilt  sensor  in  Fluxgate  compass 

Orientation  angle 

Pitch  and  roll  angle 

Disturbance  by  other  magnetic 
field  and  inclination 

Electrical  compass 

Tilt  sensor  in  Electrical  compass 

Orientation  angle 

Pitch  and  roll  angle 

High  voltage  wires,  large  metal 
structures  and  inclination 

Doppler  sensor 

Velocity 

Strength  of  the  return  signal,  field  of  the  view 

Longitudinal  accelerometer 
Lateral  accelerometer 

Yaw  angle  rate  sensor 

Pitch  angle  rate  sensor 

Roll  angle  rate  sensor 

Longitudinal  acceleration 
Lateral  acceleration 

Yaw  angle  rate 

Pitch  angle  rate 

Roll  angle  rate 

Bias  and  misalignment 
of  the  unit's  axes 

Table  1:  Localization  Sensor  Set  and  Fault  Sources 


3  Parity  Relation  Based  FDIR 
Scheme 

This  research  uses  the  FDIR  scheme  based  on  parity 
relations  combined  with  the  fused  data  and  selected 
sensor  measurements.  A  bank  of  residuals  using  these 
parity  relations  is  then  generated.  A  fault  in  any  one  of 
the  ten  sensors  causes  a  unique  subset  of  these  residu¬ 
als  to  grow,  which  allows  the  fault  to  be  detected  and 
isolated. 


3.1  Parity  Relations  Based  on  Position  and 
Orientation 

Integrating  multiple  sensors  into  the  process  is  time 
efficient  and  can  provide  more  reliable  sensing  infor¬ 
mation.  It  therefore  improves  the  reliability  of  the 
system.  The  Kalman  filter  is  used  in  this  research 
for  fusing  sensor  outputs  from  the  compasses  with 
the  odometry  estimates  employing  wheel  encoders. 
Kalman  Filtering  [11,  12]  is  a  well  known  technique 
for  state  and  parameter  estimation.  There  are  two 
compasses  used  in  this  project;  one  is  a  fluxgate  com¬ 
pass  and  the  other  is  an  electrical  compass.  They  are 
added  to  overcome  the  wheel  slippage  problem  re¬ 
lated  to  the  odometry  method.  In  the  fusion  algo¬ 
rithm,  the  wheel  encoders  propagate  the  trajectory 
state  vector  made  up  of  longitudinal  and  lateral  po¬ 
sition  and  orientation.  The  two  compasses  provide 
redundant  orientation  measurements,  which  the  filter 
uses  to  calculate  corrections  to  the  trajectory  state  and 
estimate  the  wheel  encoder  sensor  errors. 


3.1.1  Longitudinal  and  Lateral  Position 

Assuming  knowledge  of  the  initial  position,  the  lon¬ 
gitudinal  and  lateral  position  of  the  vehicle  can  be  ob¬ 
tained  by  three  different  methods: 

1.  Fusion  of  the  wheel  encoders  and  fluxgate  com¬ 
pass  using  a  Kalman  filter 


2.  Fusion  of  the  wheel  encoders  and  fluxgate  com¬ 
pass  using  a  Kalman  filter 

3.  Integration  of  the  longitudinal  or  lateral  accelera¬ 
tion  from  the  Inertial  Navigation  System  (INS) 

The  kinematical  model  which  is  used  by  the  two 
Kalman  filters  is  as  follows: 


Zfc+1  =  xk  +  T(vtotsmOk+i), 

(1) 

Uk+1  =Vk-  T(vtot  COS0fc+i), 

(2) 

0fc+i  —  —  TO, 

(3) 

■  Vr-VL 

d=  l  ’ 

(4) 

VR  +  vL 

(5) 

vtot  —  2 

where  x  denotes  the  lateral  displacement,  y  denotes 
the  longitudinal  displacement,  0  is  the  orientation  an¬ 
gle,  0  is  the  yaw  rate,  vtot  is  the  velocity  of  the  cen¬ 
ter  of  the  vehicle,  vr  and  u^are  the  direct  outputs  of 
the  right  and  left  wheel  encoders,  l  (=  0.55m)  de¬ 
notes  the  distance  between  the  two  drive  wheels  and 
T  stands  for  the  sampling  period.  It  is  well  known 
that  heading  errors  cause  large  lateral  position  errors, 
which  increase  proportionally  with  the  distance  trav¬ 
elled  by  the  vehicle.  Orientation  compasses,  which  in¬ 
clude  a  fluxgate  compass  and  an  electrical  compass, 
can  be  applied  to  measure  the  orientation  of  the  vehi¬ 
cle  and  fuse  respectively  with  the  orientation  angle  es¬ 
timated  by  the  wheel  encoders  to  overcome  the  prob¬ 
lem  of  accumulation  of  errors. 

The  INS  includes  three  accelerometers  and  three  gy¬ 
roscopes.  They  provide  the  acceleration,  pitch,  roll 
and  yaw  angular  rate  information  respectively,  in  the 
body  frame  (Figure  1).  The  longitudinal  and  lateral  ac¬ 
celerometers  in  the  INS  are  used  for  velocity  and  posi¬ 
tion  information.  Integrating  the  direct  outputs  from 
the  two  accelerometers  twice  provides  the  needed  po¬ 
sition  information.  Pitch,  roll  and  yaw  angle  rate  in¬ 
formation  are  provided  by  the  three  gyroscopes.  The 
errors  caused  by  bias  in  the  sensor  readings  accumu¬ 
late  with  time  and  inaccurate  readings  are  caused  by 
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the  misalignment  of  the  unit's  body  frame  with  re¬ 
spect  to  the  local  navigation  frame. 

Here  we  define  zq  as  the  direct  output  of  the  left 
wheel  encoder,  z\  and  z4  as  the  fused  longitudinal 
and  lateral  information  from  two  wheel  encoders  and 
fluxgate  compass  respectively,  z2  and  z$  as  the  fused 
longitudinal  and  lateral  information  from  two  wheel 
encoders  and  electrical  compass  respectively,  £3  and 
zq  as  the  integrated  data  from  longitudinal  and  lateral 
accelerometers  in  the  INS  respectively  Equations  (6) 
and  (7)  show  that  the  right  and  left  velocities  may  be 
calculated  respectively  as: 

vl  =  vtot  ~  ^0,  (6) 

vr  =  vtot  +  —0.  (7) 

where  vl  and  vr  denote  the  calculated  left  and 
right  wheel  encoder  values  respectively.  By  compar¬ 
ing  the  calculated  values  to  the  real  outputs  from  both 
the  encoders  it  is  possible  to  identify  which  one  is 
at  fault.  The  following  four  residuals  based  on  par¬ 
ity  equations  between  the  outputs  of  the  two  Kalman 
filters  and  the  INS  are  calculated  by  using  different 
combinations  of  the  above  longitudinal  and  lateral 
position  signals: 


ro  =  z0  -  vr,  (8) 

vi  =  max(\zi  -  z2\,  1 24  ~  25 1),  (9) 

r2  =  max(\zi  -  z3 1,  \z4  -  z6 1),  (10) 

r3  =  max(\z2  -  23|,  \z&  -  z6 1).  (11) 


Table  2  can  then  be  used  to  detect  a  fault  in  any  one 
of  the  position  sensor  or  sensor  groups. 


Fault  Sensor 

fo 

IT 

r 2 

*3 

Left  Wheel  Encoders 

H 

H 

H 

H 

Right  Wheel  Encoders 

L 

H 

H 

H 

Fluxgate  Compass 

L 

H 

H 

L 

Electrical  Compass 

L 

H 

L 

H 

INS 

L 

L 

H 

H 

Table  2:  Truth  Table  for  Wheel  Encoder  Fault  Detec¬ 
tion 

3.1.2  Vehicle  Orientation 

The  orientation  of  the  vehicle  can  be  obtained  by  three 
different  methods: 

1.  Fusion  of  the  wheel  encoders  and  the  fluxgate 
compass  based  on  a  Kalman  filter 

2.  Fusion  of  the  wheel  encoders  and  the  electrical 
compass  based  on  a  Kalman  filter 


3.  Integration  of  the  Yaw  angle  rate  sensor  informa¬ 
tion  in  the  INS 

The  kinematic  equations  which  are  the  basis  of  the 
Kalman  filter  used  in  the  second  method  are  Equa¬ 
tions  (l)-(3). 

The  following  three  residuals  based  on  parity  equa¬ 
tions  between  the  outputs  of  the  multiple  Kalman  fil¬ 
ters  and  yaw  angle  rate  sensor  are  calculated  by  using 
different  combinations  of  the  above  orientation  sig¬ 
nals: 

*•4  =  z7  -  z8,  (12) 

f5  =  z7  -  Zq,  (13) 

r6  =  Z8  ~  Zq.  (14) 

Where  z 7  denotes  the  fused  orientation  information 

from  two  wheel  encoders  and  the  fluxgate  compass, 
z3  is  the  fused  orientation  information  from  two  wheel 
encoders  and  the  electrical  compass  and  zg  is  the  inte¬ 
grated  data  from  the  yaw  angle  rate  sensor  in  the  INS. 
Table  3  can  then  be  used  to  detect  a  fault  in  any  one  of 
the  three  orientation  sensors. 


Faulty  Sensor 

r4 

15 

r6 

Fluxgate  Compass 

H 

H 

L 

Electrical  Compass 

H 

L 

H 

Gyroscope 

L 

H 

H 

Table  3:  Truth  Table  for  Orientation  Sensor  Fault  De¬ 
tection 


3.2  Parity  Relations  Based  on  Velocity 

Here  the  velocity  denotes  the  velocity  of  the  center  of 
the  robot  and  is  based  on  a  3-axis  coordinate  frame 
aligned  along  the  vehicle  body  (see  Figure  1).  The 
maximum  velocity  is  1  m/s. 


3.2.1  Vehicle  Velocity 

The  velocity  of  the  vehicle  can  be  obtained  by  three 
different  methods  as  follows: 

1.  The  two  wheel  encoders  using  (5) 

2.  Integration  of  the  longitudinal  accelerometer  in¬ 
formation  in  the  INS 

3.  The  Doppler  sensor 
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(19) 


In  the  third  method,  the  Doppler  non-contact  speed 
measurement  using  the  Delta  speed  sensor  is  achieved 
through  the  use  of  Doppler  radar. 

The  following  three  residuals  are  therefore  used  as 
the  bases  for  the  velocity  parity  relations: 

H  =  zio  -  *11,  (15) 

rg  =  210  -  212,  (16) 

r9  =  zu  -  z12,  (17) 

where  210  is  the  velocity  determined  from  the  two 

wheel  encoders  ,  zn  is  the  velocity  obtained  by  in¬ 
tegrating  the  data  from  longitudinal  accelerometer  in 
the  INS,  and  z\2  is  the  velocity  determined  by  the 
Doppler  sensor.  Table  4  can  then  be  used  to  detect  a 
fault  in  any  one  of  the  three  velocity  sensors  or  sensor 
groups. 


Faulty  Sensor 

*7 

*8 

*9 

Wheel  Encoders 

H 

H 

L 

INS 

H 

T 

H 

Doppler  Sensor 

T 

H 

H 

Table  4:  Truth  Table  for  Velocity  Sensor  Fault  Detec¬ 
tion 

3.3  Parity  Relations  Based  on  Attitude 
Sensors 

Attitude  denotes  the  roll  and  pitch  angles  of  the  vehi¬ 
cle.  Here  the  attitude  is  aligned  with  the  3-axis  body 
coordinates  (see  Figure  1).  Three  groups  of  sensors 
supply  attitude  measurements:  the  INS,  the  fluxgate 
compass  and  the  electrical  compass.  Both  compasses 
measure  roll  and  pitch  angles  in  the  range  of  degrees 
[-45  45]. 

3.3.1  Vehicle  Pitch  Angle 

The  pitch  angle  of  the  vehicle  can  be  obtained  by  three 
different  methods  as  described  follows: 

1 .  Integration  of  the  pitch  angle  rate  sensor  informa¬ 
tion  in  the  INS 

2.  Pitch  output  from  the  fluxgate  compass 

3.  Pitch  output  from  the  electrical  compass 

The  following  three  residuals  are  the  bases  for  ve¬ 
locity  parity  equations  among  the  outputs  of  the 
above  pitch  angle  signals: 

no  =  Z13  -  Z14,  (18) 


Hi  =  213  —  215, 

H 2  =  214  -  215,  (20) 


Where  z13  denotes  the  integrated  data  from  pitch 
angle  rate  sensor  in  INS,  214  is  the  pitch  output  from 
fluxgate  compass  and  215  is  the  pitch  output  from 
electrical  compass.  Table  5  can  then  be  used  to  detect 
a  fault  in  any  one  of  the  three  pitch  angle  sensors. 


Faulty  Sensor 

Ho 

*12 

Pitch  Gyroscope 

H 

H 

T 

Fluxgate  Compass 

H 

T 

H 

Electrical  Compass 

T 

H 

H 

Table  5:  Truth  Table  for  Pitch  Attitude  Sensor  Fault 
Detection 


3.3.2  Vehicle  Roll  Angle 

Table  6  can  then  be  used  to  detect  a  fault  in  any  one  of 
the  three  roll  angle  sensors  using  the  same  method. 


Faulty  Sensor 

*13 

*14 

*15 

Roll  Gyroscope 

H 

H 

L 

Fluxgate  Compass 

H 

L 

H 

Electrical  Compass 

L 

H 

H 

Table  6:  Truth  Table  for  Roll  Attitude  Sensor  Fault  De¬ 
tection 

4  A  System  for  Automated  Fault 
Detection  and  Isolation 

Table  7  summarizes  18  different  output  signals  used  in 
the  vehicle  navigation  system.  Some  are  directly  mea¬ 
sured,  while  the  others  are  fused  using  Kalman  filters. 
Table  8  summarizes  15  different  residuals  calculated 
using  combinations  of  the  output  signals  from  Table 
7.  It  includes  longitudinal  and  lateral  position,  orien¬ 
tation,  longitudinal  velocity  and  attitude  information. 

By  processing  the  above  15  different  residuals,  it  is 
possible  to  identify  a  fault  in  any  of  the  sensors.  Table 
9  shows  how  a  fault  in  any  one  of  the  sensors  causes 
a  unique  combination  of  residuals  to  grow.  The  cor¬ 
responding  faulty  sensor  can  then  be  detected  and 
identified. 
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Output 

Description 

Sensors  Related 

■Zl 

Robot 

Wheel  encoders,  fluxgate 

22 

longitudinal 

compass 

Wheel  encoders,  electrical 

23 

position 

compass 

Longitudinal  accelerome¬ 
ter  in  INS 

^4 

Robot 

Wheel  encoders,  fluxgate 

25 

lateral 

compass 

Wheel  encoders,  electrical 

26 

position 

compass 

Lateral  accelerometer  in 
INS 

27 

Robot 

Wheel  encoders,  fluxgate 

28 

heading 

compass 

Wheel  encoders,  electrical 

29 

angle 

compass 

Yaw  angle  rate  sensor  in 
INS 

210 

Robot 

Wheel  encoders 

2ll 

longitudinal 

Longitudinal  accelerome¬ 
ter  in  INS 

212 

velocity 

Non-contact  Doppler  sen¬ 

sor 

213 

Robot 

Pitch  angle  rate  sensor  in 
INS 

214 

215 

pitch 

attitude 

Fluxgate  compass 

Electrical  compass 

216 

Robot 

Roll  angle  rate  sensor  in 
INS 

217 

218 

roll 

attitude 

Fluxgate  compass 

Electrical  compass 

Table  7:  Bank  of  Signals  for  Fault  Detection  and  Iden¬ 
tification  Scheme 


5  Simulation  Results 

The  fault  detection  and  identification  scheme  de¬ 
signed  in  the  previous  sections  was  simulated  to  test 
its  performance.  We  apply  it  to  navigation  of  the 
ATRV-Jr.  in  a  dense  forest  using  a  multivalued  fuzzy 
behavior  control  algorithm  [6].  The  algorithm  realizes 
goal  seeking,  and  obstacle  avoidance  functions.  The 
goal  seeking  behavior  directs  the  robot  to  a  specific 
predefined  target.  The  objective  of  the  goal  seeking 
behavior  is  to  make  the  difference  between  the  robot 
heading  direction  and  the  goal  direction  as  small  as 
possible.  The  obstacle  avoidance  behavior  for  the  ve¬ 
locity  control  activity  uses  the  minimum  distance  to 
the  nearest  obstacle  and  the  current  velocity  to  deter¬ 
mine  the  amount  by  which  the  velocities  of  the  two 
wheels  should  be  changed.  When  the  robot  has  some 
localization  sensors  at  fault,  the  FDIR  scheme  can  de¬ 
tect  the  faulty  sensors  and  reconfigure  the  control  sys¬ 


tem  so  that  it  uses  only  non-faulty  sensors. 

For  the  simulation,  the  assumed  sensor  measure¬ 
ment  noises  were  Gaussian  white  noise.  The  ATRV- 
Jr  was  assumed  to  be  travelling  with  the  velocity  of 
the  right  and  left  wheels  around  0.53m/s  respectively 
with  the  roll  and  pitch  angles  in  the  range  of  degrees 
[-45,45]. 

5.1  Longitudinal  Position  Sensor  Fault 
Detection 

As  described  in  Subsection  3.2.1,  there  are  three  sensor 
groups  that  can  be  fused  using  Kalman  filters  to  pro¬ 
vide  longitudinal  position  data  for  the  vehicle  naviga¬ 
tion  system.  In  the  simulation,  the  default  method  for 
position  information  is  the  third  method,  that  is,  dou¬ 
ble  integration  of  the  longitudinal  accelerometer  out¬ 
puts  from  the  INS.  While  a  fault  in  the  sensor  is  de¬ 
tected,  the  scheme  will  switch  to  the  reconfiguration 
step.  In  the  simulation,  a  fault  in  the  longitudinal  ac¬ 
celerometer  was  said  to  occur  at  t  =  14s,  which  caused 
two  longitudinal  residuals  among  the  three  to  be  big 
enough  to  identify  the  fault.  When  there  was  no  FDIR 
scheme,  the  faulty  sensor  outputs  were  used  continu¬ 
ously.  The  forest  navigation  algorithm  then  operated 
based  on  faulty  information,  which  caused  the  robot 
to  fail  to  fulfill  both  obstacle  avoidance  and  goal  seek- 
ing. 
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Figure  2:  Trajectories  with  and  without  INS  Sensor 
Fault 

Figure  2  shows  the  normal  trajectory  and  the  faulty 
trajectory  with  longitudinal  accelerometer  fault  occur¬ 
ring  at  t  =  14s  without  FDIR  scheme.  It  is  shown  that 
when  there  is  no  any  fault  occurs,  the  algorithm  works 
well  and  the  robot  can  avoid  the  trees  and  arrive  at  the 
desired  destination.  When  the  default  position  sensor 
fault  occurs  from  the  14  second  without  detection,  the 
robot  hits  one  or  more  trees  in  about  4  sec  and  stop 
and  therefore  lose  track  of  the  goal.  Figure  3  presents 
a  comparison  of  the  two  trajectories  with  and  without 
the  FDIR.  With  FDIR,  the  faulty  accelerometer  can  be 
detected  and  replaced  with  fused  position  data  from 
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Faulty  Sensor 

r 2 

*3 

r4 

*5 

r6 

r? 

r8 

r9 

rio 

rn 

ri2 

ri3 

ri4 

ris 

Wheel  Encoders 

H 

H 

H 

H 

H 

H 

H 

H 

L 

L 

L 

L 

L 

L 

L 

Fluxgate  heading  compass 

H 

H 

L 

H 

H 

L 

L 

L 

L 

H 

L 

H 

H 

L 

H 

Electrical  heading  compass 

H 

L 

H 

H 

L 

H 

L 

L 

L 

L 

H 

H 

L 

H 

H 

Lateral  accelerometer 

L 

H 

H 

L 

L 

L 

L 

L 

L 

L 

L 

L 

L 

L 

L 

Longitudinal  accelerometer 

L 

H 

H 

L 

L 

L 

H 

L 

H 

L 

L 

L 

L 

L 

L 

Yaw  angle  rate  sensor 

L 

L 

L 

L 

H 

H 

L 

L 

L 

L 

L 

L 

L 

L 

L 

Non-contact  Doppler  sensor 

L 

L 

L 

L 

L 

L 

L 

H 

H 

L 

L 

L 

L 

L 

L 

Pitch  angle  rate  sensor 

L 

L 

L 

L 

L 

L 

L 

L 

L 

H 

H 

L 

L 

L 

L 

Roll  angle  rate  sensor 

L 

L 

L 

L 

L 

L 

L 

L 

L 

L 

L 

L 

H 

H 

L 

Table  9:  Complete  Truth  Table  for  Navigation  Sensor  Fault  Detection 


the  fluxgate  compass  and  the  wheel  encoders.  The  re-  achieve  its  goal, 
configuration  allows  the  robot  to  avoid  the  trees  and 


140 

120 

100 

80 

60 

40 

20 

0 


-20 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 

-20  0  20  40  60  80  100  120  140  160 

Figure  3:  Trajectories  with  and  without  Position  FDIR 
with  INS  Sensor  Fault 


X  Position  Comparison  with  INS  Fault  Occurs  trom  the  14th  Second 


Figure  4:  Longitudinal  Position  Outputs  and  Three 
Residual  Comparisons  with  INS  Fault 


Figure  4  shows  the  comparison  of  the  longitudinal 
position  outputs  from  the  three  different  methods  and 
the  values  of  the  residuals  r\,  r<z  and  r%  respectively 
with  the  longitudinal  accelerometer  fault  occurrence 
from  the  14th  second  (see  Table  2).  It  is  clearly  seen 
that  fused  outputs  from  the  two  Kalman  filters  are 
similar  to  each  other  while  the  outputs  from  the  INS 
is  different  from  the  two  others  from  the  14th  second. 
Correspondingly,  only  residue  r\  is  low  which  indi¬ 
cates  that  the  sensor  in  the  INS  is  at  fault  and  the  faulty 
sensor  is  detected  and  identified. 

The  simulation  results  for  lateral  position  and  orien¬ 
tation  sensor  FDIR  were  also  obtained.  They  are  simi¬ 
lar  to  that  of  the  longitudinal  position  sensor  FDIR. 


5.2  Velocity  Sensor  Fault  Detection 

Three  groups  of  sensors  provide  velocity  information 
for  the  measurement  of  the  dynamic  motion  of  the 
robot  as  described  in  Subsection  3.2.1.  When  no  ve¬ 
locity  sensor  fault  occurs,  the  second  method  is  the 
default  approach,  that  is  integrating  the  output  once 
from  the  longitudinal  accelerometer  in  the  INS.  Figure 
5  shows  the  situation  when  the  default  INS  sensor  is 
at  fault  from  the  14th  sec.  The  robot  cannot  avoid  the 
obstacles.  It  hits  the  tree  and  stops  in  about  4  sec  and 
will  not  successfully  arrive  at  the  destination  without 
FDIR.  While  with  the  fault  detection  and  identifica¬ 
tion,  the  faulty  sensor  output  will  be  replaced  by  the 
Doppler  sensor  for  the  velocity  information.  It  is  seen 
that  the  robot  can  avoid  the  obstacle  and  thereafter 
reach  the  destination  Figure  6  shows  the  comparison 
of  three  velocity  outputs  and  the  values  of  three  resid¬ 
uals  7*7,  r8  and  r9  of  Table  4  during  the  INS  sensor 
fault.  It  is  clear  that  only  residue  r8  is  low  which  in- 
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dicates  that  the  INS  sensor  is  the  faulty  component. 


Figure  5:  Trajectories  with  and  without  Velocity  FDIR 
with  the  INS  Fault 


Figure  6:  Velocity  Outputs  and  Three  Residual  Com¬ 
parisons  with  INS  Fault 


6  Conclusion 

This  paper  develops  an  effective  parity  equation 
based  fault  detection  and  identification  system  pro¬ 
viding  a  methodology  to  continuously  monitor  all 
the  sensors  used  in  the  vehicle  navigation  to  ensure 
the  system  health,  excluding  Differential  GPS.  Mean¬ 
time  the  Kalman  filter  based  fusion  method  provides 
more  reliable  information.  The  scheme  was  shown 
to  work  well  when  simulated  with  our  detailed  re¬ 
search  robot  model.  Experimental  implementation 
of  the  entire  fault  detection  and  identification  system 
on  the  ATRV-Jr.  is  expected  to  occur  in  the  near  future. 
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